/*************************************************************************************************************
*                                                    UTOS
*                               A non-preemptive(cooperative) multitasking RTOS Kernel
*
* Copyright (c) 2013, HuangLiang (huangliang.tj@gmail.com)
* Under New BSD license, see "LICENSE" for detail
*
* @.file       utos_thread.c
* @.version    refer to macro "UT_VERSION" in utos.h
* @.brief      
* 
*************************************************************************************************************/
#include  "utos.h"
static  UT_TCB * UT_Thread_New(UT_PCB  *p_proc, UT_INT8U thrd_prio, UT_INT8U *err);
static  UT_INT8U UT_Thread_Free(UT_TCB *p_thrd);

static  UT_TCB * UT_Thread_New(UT_PCB  *p_proc, UT_INT8U thrd_prio, UT_INT8U *err)
{
    // Must make sure p_proc is not NULL before call this function!!!
    UT_TCB *p_thrd;
    if(!UT_TCB_Free_Entry)
    {
        *err = UT_ERR_THRD_FULL;                    // If empty, UT_TCB_Free_Entry will never be null
        return (UT_TCB*)0;
    }
    
    if((p_proc->opt & UT_PCB_OPT_DYN_PQ) == 0)      // Check for dynamic pq of a process
    {
        if(p_proc->tcb_cnt >= (p_proc->tpq_size - 1))
        {
            *err = UT_ERR_PROC_PQ_NOT_DYN;      // The process does not have a dynamic pq, can't add thread 
            return (UT_TCB*)0;             
        }
    }

    p_thrd = UT_TCB_Free_Entry;    // Get a new thread
    UT_TCB_Free_Entry = UT_TCB_Free_Entry->p_next;
    
    p_thrd->p_prev = p_proc->p_tcb_list;            // Attach to Process list
    p_thrd->p_next = (UT_TCB*)0;
    if(p_proc->p_tcb_list)
        p_proc->p_tcb_list->p_next = p_thrd;
    p_proc->p_tcb_list = p_thrd;            // shift
    
    p_proc->tcb_cnt++;
    p_thrd->p_pcb             = p_proc;
    
    UT_TCB_Created_Num++;
    
    *err = UT_ERR_NONE;
    return p_thrd;
}
static  UT_INT8U UT_Thread_Free(UT_TCB *p_thrd)
{
    // Must make sure p_thrd is not Null before call this function
    UT_PCB *p_proc;
    p_proc = p_thrd->p_pcb;
    if(!p_proc)
        return UT_ERR_PROC_PRIO_NOT_EXIST;
    if(p_thrd == p_proc->p_tcb_list)                    // tail, move it
    {
        p_proc->p_tcb_list = p_proc->p_tcb_list->p_prev;
        if(p_proc->p_tcb_list)
            p_proc->p_tcb_list->p_next = (UT_TCB*)0;
    }
    else
    {
        p_thrd->p_next->p_prev = p_thrd->p_prev;
        if(p_thrd->p_prev)
            p_thrd->p_prev->p_next = p_thrd->p_next;
    }
    
    UT_MemClr(p_thrd, sizeof(UT_TCB));
    
    p_thrd->p_next = UT_TCB_Free_Entry;
    p_thrd->p_prev = (UT_TCB*)0;
    if(UT_TCB_Free_Entry)
        UT_TCB_Free_Entry->p_prev = p_thrd;
    UT_TCB_Free_Entry = p_thrd;
    p_proc->tcb_cnt--;
    UT_TCB_Created_Num--;
    return UT_ERR_NONE;
}
/*************************************************************************************************************
*                                         Global:  UT_Thread_Init()
*
* @.brief     Initial TCB block
*
* @.argument  none.
*
* @.return    none.
*************************************************************************************************************/
void        UT_Thread_Init(void)
{
    int i;
    UT_TCB *p1,*p2;
    UT_MemClr(&UT_TCB_Pool[0], sizeof(UT_TCB_Pool));
    p1 = &UT_TCB_Pool[0];
    p2 = &UT_TCB_Pool[1];
    
    p1->p_prev  = (UT_TCB*)0;
    for(i = 0;i < UT_THREAD_MAX-1;i++)
    {
        p1->p_next = p2;
        p2->p_prev = p1;
        p2->p_next = (UT_TCB*)0;
        p1++;
        p2++;
    }

    UT_TCB_Free_Entry = &UT_TCB_Pool[0];
    UT_TCB_Created_Num = 0;
    
    UT_TCB_Runtime_Cur = (UT_TCB*)0;            // Runtime pointer set to Null
}
/*************************************************************************************************************
*                                         Global:  UT_Thread_Create()
*
* @.brief     
*
* @.argument  
*
* @.return    .
*************************************************************************************************************/
UT_INT8U    UT_Thread_Create(UT_INT8U proc_prio, UT_INT8U thrd_prio, 
                            void* (*thrd_cb_func)(void*), UT_INT32U thrd_timeout, void * thrd_arg)
{
    UT_PCB  *p_proc;
    UT_TCB  *p_thrd;
    UT_INT8U err;
    
    p_proc =  UT_Process_Find_Proc_Prio (proc_prio);
    if(!p_proc)
        return UT_ERR_PROC_PRIO_NOT_EXIST;
        
    if(thrd_prio>UT_THREAD_LOWEST_PRIO)
        return UT_ERR_THRD_PRIO_INVALID;
    if(!thrd_cb_func)
        return UT_ERR_P_FUNC_NULL;
    
    p_thrd = UT_Thread_New(p_proc, thrd_prio, &err);
/*    if(!UT_TCB_Free_Entry)
        return UT_ERR_THRD_FULL;                    // If empty, UT_TCB_Free_Entry will never be null
    
    if((p_proc->opt & UT_PCB_OPT_DYN_PQ) == 0)
    {
        if(p_proc->tcb_cnt >= (p_proc->tpq_size - 1))
            return UT_ERR_PROC_PQ_NOT_DYN;              // The process does not have a dynamic pq, can't add thread 
    }

    p_thrd = UT_TCB_Free_Entry;    // Get a new thread
    UT_TCB_Free_Entry = UT_TCB_Free_Entry->p_next;
    
    p_thrd->p_prev = p_proc->p_tcb_list;            // Attach to Process list
    p_thrd->p_next = (UT_TCB*)0;
    if(p_proc->p_tcb_list)
        p_proc->p_tcb_list->p_next = p_thrd;
    p_proc->p_tcb_list = p_thrd;            // shift
    
    p_proc->tcb_cnt++;
    
    p_thrd->p_pcb             = p_proc;
*/
    if(p_thrd)
    {
        p_thrd->p_event           = (UT_EVENT*)0;
        p_thrd->timeout           = thrd_timeout;
        p_thrd->timeout_reg       = thrd_timeout;
        p_thrd->thread_callback   = thrd_cb_func;
        p_thrd->p_thread_arg      = (void*)thrd_arg;        // Initial to null
        p_thrd->p_thread_rtn      = (void*)0;               // Initial to null
        
        p_thrd->prio              = thrd_prio;
        p_thrd->stat              = UT_TCB_STAT_TIMEOUT;      // waiting for timeout. 
        
    }
    // pq push?
    return err;
}
/*************************************************************************************************************
*                                         Global:  UT_Thread_Delete()
*
* @.brief     
*
* @.argument  
*
* @.return    .
*************************************************************************************************************/
UT_INT8U    UT_Thread_Delete(UT_INT8U proc_prio, UT_INT8U thrd_prio)
{
    UT_TCB  *p_thrd;
    UT_INT8U  rtn;;

    p_thrd =  UT_Thread_Find_Thrd_Prio(proc_prio, thrd_prio);
    if(!p_thrd)
        return UT_ERR_THRD_PRIO_NOT_EXIST;
    
    rtn = UT_Thread_Free(p_thrd);
    return rtn;
        
/*    if(p_thrd == p_proc->p_tcb_list)                    // tail, move it
    {
        p_proc->p_tcb_list = p_proc->p_tcb_list->p_prev;
        if(p_proc->p_tcb_list)
            p_proc->p_tcb_list->p_next = (UT_TCB*)0;
    }
    else
    {
        p_thrd->p_next->p_prev = p_thrd->p_prev;
        if(p_thrd->p_prev)
            p_thrd->p_prev->p_next = p_thrd->p_next;
    }
    
    UT_MemClr(p_thrd, sizeof(UT_TCB));
    
    p_thrd->p_next = UT_TCB_Free_Entry;
    p_thrd->p_prev = (UT_TCB*)0;
    if(UT_TCB_Free_Entry)
        UT_TCB_Free_Entry->p_prev = p_thrd;
    UT_TCB_Free_Entry = p_thrd;
    p_proc->tcb_cnt--;
    UT_TCB_Created_Num--;
    return UT_ERR_NONE;
*/
}
UT_INT8U    UT_Thread_Delete_Self(void)
{
    return UT_Thread_Delete(UT_PRIO_PROC_SELF, UT_PRIO_THRD_SELF);
}
UT_TCB  * UT_Thread_Find_Thrd_Prio(UT_INT8U proc_prio, UT_INT8U thrd_prio)
{
    UT_PCB  *p_proc;
    UT_TCB  *p_thrd;
    
    if(thrd_prio == UT_PRIO_THRD_SELF)
        return UT_TCB_Runtime_Cur;                  // Speed up run-time search
        
    if(thrd_prio>UT_THREAD_LOWEST_PRIO)
        return (UT_TCB*)0;

    p_proc = UT_Process_Find_Proc_Prio(proc_prio);
    if(!p_proc)
        return (UT_TCB*)0;

    p_thrd = p_proc->p_tcb_list;
    
    while(p_thrd)                                   // Find in linear time
    {
        if(p_thrd->prio == thrd_prio)
            break;
        p_thrd = p_thrd->p_prev;                    //p_proc->p_tcb_list is the tail of TCBs attached to this PCB
    }

    return p_thrd;                                  // If not exist, may return Null
    
}
UT_INT8U    UT_Thread_Arg_Set(UT_INT8U proc_prio, UT_INT8U thrd_prio, void * thrd_arg)
{
    UT_TCB *p_tcb;
    p_tcb = UT_Thread_Find_Thrd_Prio(proc_prio, thrd_prio);
    if(!p_tcb)
        return UT_ERR_THRD_PRIO_NOT_EXIST;
    p_tcb->p_thread_arg = thrd_arg;
    return UT_ERR_NONE;
}
UT_TCB *   UT_Thread_Fork(UT_INT8U *err)    // Using current running thread to duplicate a new one
{
    UT_INT8U __err;
    UT_TCB * p_thrd;
    if(!UT_TCB_Runtime_Cur)
    {
        *err = UT_ERR_SYS_NOT_RUNNING;
        return (UT_TCB*)0;
    }
    p_thrd = UT_Thread_New(UT_TCB_Runtime_Cur->p_pcb, UT_TCB_Runtime_Cur->prio, &__err);
    if(p_thrd)
    {
        p_thrd->p_event           = (UT_EVENT*)0;
        p_thrd->timeout           = UT_TCB_Runtime_Cur->timeout_reg;
        p_thrd->timeout_reg       = UT_TCB_Runtime_Cur->timeout_reg;
        p_thrd->thread_callback   = UT_TCB_Runtime_Cur->thread_callback;
        p_thrd->p_thread_arg      = UT_TCB_Runtime_Cur->p_thread_arg;
        p_thrd->p_thread_rtn      = (void*)0;               // Initial to null
        
        p_thrd->prio              = UT_TCB_Runtime_Cur->prio;
        p_thrd->stat              = UT_TCB_STAT_TIMEOUT;      // Clear to waiting for timeout. 
    }
    *err = __err;
    return p_thrd;
}
UT_TCB *   UT_Thread_Fork_2(UT_INT8U *err, void* (*thrd_cb_func)(void*))
{
    UT_INT8U __err;
    UT_TCB * p_thrd;
    p_thrd = UT_Thread_Fork(&__err);
    if(p_thrd)
    {
        p_thrd->thread_callback = thrd_cb_func;
    }
    *err = __err;
    return p_thrd;
}
UT_TCB *   UT_Thread_Fork_3(UT_INT8U *err, void* (*thrd_cb_func)(void*), UT_INT32U thrd_timeout)
{
    UT_INT8U __err;
    UT_TCB * p_thrd;
    p_thrd = UT_Thread_Fork(&__err);
    if(p_thrd)
    {
        p_thrd->thread_callback = thrd_cb_func;
        p_thrd->timeout         = thrd_timeout;
        p_thrd->timeout_reg     = thrd_timeout;
    }
    *err = __err;
    return p_thrd;
}
UT_INT8U  UT_Thread_Sleep(UT_INT8U proc_prio, UT_INT8U thrd_prio)
{
    UT_TCB * p_thrd;
    p_thrd = UT_Thread_Find_Thrd_Prio(proc_prio, thrd_prio);
    if(!p_thrd)
        return UT_ERR_THRD_PRIO_NOT_EXIST;
    // here we will not judge if this p_thrd->p_pcb
    p_thrd->stat  |=  UT_TCB_STAT_SLEEP;
    return UT_ERR_NONE;
}
UT_INT8U  UT_Thread_Awake(UT_INT8U proc_prio, UT_INT8U thrd_prio)
{
    UT_TCB * p_thrd;
    p_thrd = UT_Thread_Find_Thrd_Prio(proc_prio, thrd_prio);
    if(!p_thrd)
        return UT_ERR_THRD_PRIO_NOT_EXIST;
    // here we will not judge if this p_thrd->p_pcb
    p_thrd->stat  &=  ~UT_TCB_STAT_SLEEP;
    return UT_ERR_NONE;
}
/*************************************************************************************************************
*                                                  END
*************************************************************************************************************/
